#define ZRSIMULATION
/* 
 * gsp.c
 *
 * SPHERES Guest Scientist Program custom source code template.
 *
 * MIT Space Systems Laboratory
 * SPHERES Guest Scientist Program
 * http://ssl.mit.edu/spheres/
 * 
 * Copyright 2005 Massachusetts Institute of Technology
 */


/*----------------------------------------------------------------------------*/
/*                         Do not modify this section.                        */
/*----------------------------------------------------------------------------*/

#include "comm.h"
#include "commands.h"
#include "control.h"
#include "gsp.h"
#include "gsp_task.h"
#include "pads.h"
#include "prop.h"
#include "spheres_constants.h"
#include "spheres_physical_parameters.h"
#include "spheres_types.h"
#include "std_includes.h"
#include "system.h"
#include "util_memory.h"

/*----------------------------------------------------------------------------*/
/*                     Modify as desired below this point.                    */
/*----------------------------------------------------------------------------*/
#include <math.h>
#include <string.h>
#include "pads_internal.h"
#include "find_state_error.h"
#include "ctrl_attitude.h"
#include "ctrl_position.h"
#include "ctrl_mix.h"
#include "ctrl_avoid.h"
#include "ZRGame.h"
#include "ZRGameInternal.h"
#include "ZR_API_internal.h"
#include "ZR_API.h"
#include "control_internal.h"
#include "gspZR.h"
#include "math_matrix.h"

//Function pointers to user game and init code
ZR_USR_FCN ZRUser;
ZR_INIT_FCN ZRInit;

//ID of this team (to be appended to test result number)
static int teamId = -1;

//Local function for assiging function pointers
static void assignFcnPtrs(unsigned char id);
static unsigned char breakTieResult(unsigned char defaultVal);

float sign(float a);
extern const float KPvelocityP;
unsigned char panelTorqueCounts;
unsigned char torqueOnCounts;
unsigned char lastHavePanel;
float panelTorqueDir = 1.0f;
static unsigned char fcnsAssigned = 0;


//=============================================
//				Configurable Options
//=============================================

#if SPHERE_ID == SPHERE1
static float initQuat[4] = {0.707f,-0.707f,0.0f,0.0f};
#else
static float initQuat[4] = {0.707f,0.707f,0.0f,0.0f};
#endif

float refVector[3] = {-1, 0, 0};

void gspInitTestZR(unsigned int test_number)
{
	
	
#if (SPHERE_ID == SPHERE1)
	state_vector initState = {0.3f,0.0f,0.0f,  0.0f,0.0f,0.0f,  0.0f,1.0f,0.0f,0.0f,  0.0f,0.0f,0.0f};
#else
	state_vector initState = {-0.3f,0.0f,0.0f,  0.0f,0.0f,0.0f,  1.0f,0.0f,0.0f,0.0f,  0.0f,0.0f,0.0f};
#endif
	
	
	
#if(SPHERE_ID == SPHERE1)
	padsEstimatorInitWaitAndSet(initState, padsInertialBufferCapacity(), 200, 105, PADS_INIT_THRUST_INT_ENABLE, PADS_BEACONS_SET_1TO9);
#else
	padsEstimatorInitWaitAndSet(initState, padsInertialBufferCapacity(), SYS_FOREVER, SYS_FOREVER, PADS_INIT_THRUST_INT_ENABLE, PADS_BEACONS_SET_1TO9);
#endif		
	propSetThrusterBypassIR(FALSE);
	
	//Initialize the simulation if running on the flat floor
#ifdef ZRFLATFLOOR
	memcpy(&initState[QUAT_1], initQuat, sizeof(float)*4);
	getInitPos(&initState[POS_X]);
	sixDOFSimInit(initState);
#endif
	
	ctrlPeriodSet(1000);
	
	//Reset attitude and PID controllers
	ctrlAttitudePIDinit();	
	
	//Set fcnsAssigned to 0 to indicate that we haven't finished assinging competitor ids
	fcnsAssigned = 0;
	teamId = -1;
	
	//Initialize the ZR API and 6DOF simulator
	ZRAPIInit();
	
	//Always reset game (for clean telemetry)
	initZRGame();
	
#ifdef ZRSIMULATION
	//***** THIS ONLY AFFECTS THE SIMULATION ***/
	assignFcnPtrs(0); //In sim, direct both satellites to use team ID #0 (ZRUser01/ZRInit01)
	
	//Call user initialization code
	ZRInit();
#endif
	
	//For SPH1, we can go ahead and assign a function pointer for ZRUser and ZRInit based on the test number
#if (SPHERE_ID == SPHERE1)
	assignFcnPtrs(test_number-1);
#endif
	//Reset runtime command queue
	commRunTimeCmdReset();
	
	//Set up torque counts
	panelTorqueCounts = 0;
	lastHavePanel = 0;
	
}

void gspControlZR(unsigned int test_number, unsigned int test_time, unsigned int maneuver_number, unsigned int maneuver_time)
{
	state_vector ctrlState;
	state_vector ctrlStateTarget, ctrlStateError, ctrlStatePartner;	
	float ctrlControl[6];
	float statePosVel[12], statePosVelPartner[12];
	int minPulseWidth = 10, ii=0;
#ifndef ZRFLATFLOOR	
	float duty_cycle = 20.0f;
#else
	float duty_cycle = 40.0f;
#endif
	int collisionActive = 0;
	float collisionInfo[2];
	unsigned char clControl; //User closed loop control mode	
	float userAtt[3];
	char cmd;				//Command from crew to select satellite competitor roles
	float posPanel[4];
	prop_time firing_times;
	comm_payload_soh soh_partner;	
	dbg_short_packet DebugVecShort;
	dbg_ushort_packet DebugVecUShort;
	dbg_float_packet DebugVecFloat;
	
	unsigned char userSetAtt=0;
	
	extern float VEHICLE_MASS;
	
	// PD controller
	extern const float KPattitudePD; // Proportional gain for attitude control wn=0.400 rad/s
	extern const float KDattitudePD; // Derivative gain for attitude control wn=0.400 rad/s with 25% damping reduction
	extern const float KPpositionPD; // Proportional gain for position control wn=0.200 rad/s
	extern const float KDpositionPD; // Derivative gain for position control wn=0.200 rad/s
	
	// PID controller (the integral term is set for a time constant of approximately 20 sec)
#ifndef ZRFLATFLOOR	
	extern const float KPattitudePID; // Proportional gain for attitude control wn=0.400 rad/s
	extern const float KIattitudePID; // Derivative gain for attitude control wn=0.400 rad/s
	extern const float KDattitudePID; // Derivative gain for attitude control wn=0.400 rad/s with 25% damping reduction
#else //Copied from SPHERES_PHYSICAL_ISS
	extern const float KPpositionPID;
	extern const float KDpositionPID;
	extern const float KIpositionPID;
	const float KPattitudePID = 0.0045f; // Proportional gain for attitude control wn=0.400 rad/s
	const float KIattitudePID = 0.00018f; // Derivative gain for attitude control wn=0.400 rad/s
	const float KDattitudePID = 0.0143f; // Derivative gain for attitude control wn=0.400 rad/s with 25% damping reduction
	setMaxFuelUsePercent(0.24);
#endif
	
	memset(ctrlState,0,sizeof(ctrlState));
	memset(ctrlStatePartner,0,sizeof(ctrlStatePartner));
	memset(ctrlStateTarget,0,sizeof(ctrlStateTarget));
	memset(ctrlStateError,0,sizeof(ctrlStateError));
	memset(ctrlControl,0,sizeof(ctrlControl));
	memset(statePosVel,0,sizeof(statePosVel));
	memset(&firing_times, 0, sizeof(prop_time));
	memset(statePosVelPartner,0,sizeof(statePosVelPartner));
	memset(DebugVecShort, 0, sizeof(DebugVecShort));
	memset(DebugVecUShort, 0, sizeof(DebugVecUShort));
	memset(DebugVecFloat, 0, sizeof(DebugVecFloat));
	
	//Get state of health from other satellite
#if (SPHERE_ID == SPHERE1)
	commBackgroundSOHGet(SPHERE2, &soh_partner);
#else
	commBackgroundSOHGet(SPHERE1, &soh_partner);
#endif
	
	//Retrieve estimated state
	padsStateGetZR(&ctrlState, &ctrlStatePartner);
	
	
	
	//===========================================
	//			Main Maneuver Actions 
	//===========================================
	switch(maneuver_number)
	{
		case 1: //Kalman Filter initialization
			if(test_time > 10000){
				ctrlManeuverTerminate();
				if (!fcnsAssigned) {
					ctrlTestTerminateZR(ZR_RESULT_INVALID_COMPETITOR);
				}
			}
			
			if (commRunTimeCmdCheck()){
				cmd = commRunTimeCmdGet();
#if (SPHERE_ID == SPHERE2)
				assignFcnPtrs(getZRUserIndexFromCommand(cmd));  //Note: only SPH2 will be assigned since SPH1 is assigned in initTest
#endif
			}		
			break;
			
		case 2: //Go to pre-designated location
			
			padsGlobalPeriodSet(SYS_FOREVER);
			//Grab initialized position
			getInitPos(&ctrlStateTarget[POS_X]);
			
			memcpy(&ctrlStateTarget[QUAT_1], initQuat, sizeof(float)*4);
			
			//Get error between current state and desired position
			findStateError(ctrlStateError, ctrlState, ctrlStateTarget);
			
			//Run position and attitude controller to move satellite to target
			ctrlAttitudeNLPDwie(KPattitudePD, KDattitudePD, KPattitudePD, KDattitudePD, KPattitudePD, KDattitudePD, ctrlStateError, ctrlControl);
			ctrlPositionPD(KPpositionPD,KDpositionPD,ctrlStateError,ctrlControl);
			break;
			
			
		case 3: //Call ZR player's target function
#if (SPHERE_ID == SPHERE1)
			padsGlobalPeriodSet(SYS_FOREVER);
#endif
			
			//Assign state targets to current state in case user does not set targets
			memcpy(ctrlStateTarget, ctrlState, sizeof(state_vector));
			//Clear user attitude target
			memset(userAtt,0,sizeof(float)*3);
			
			
			//Set up states for targeting function
			memcpy(statePosVel, ctrlState, sizeof(float)*6);
			memcpy(statePosVelPartner, ctrlStatePartner, sizeof(float)*6);
			
			ZRQuat2AttVec(refVector,&ctrlState[QUAT_1],&statePosVel[6]);
			ZRQuat2AttVec(refVector,&ctrlStatePartner[QUAT_1],&statePosVelPartner[6]);
			
			statePosVel[9] = ctrlState[RATE_X];		statePosVelPartner[9] = ctrlStatePartner[RATE_X];
			statePosVel[10] = ctrlState[RATE_Y]; 	statePosVelPartner[10] = ctrlStatePartner[RATE_Y];
			statePosVel[11] = ctrlState[RATE_Z];	statePosVelPartner[11] = ctrlStatePartner[RATE_Z];
			
			//Update the Game information
			updateZRGame(maneuver_time, ctrlState, ctrlStatePartner, ZR_TIMEOUT);
			
			//===== HERE IS WHERE THE ZR Main code Loop FUNCTION IS CALLED ===			
			ZRUser(statePosVel, statePosVelPartner, ((float) maneuver_time)/1000.0f);
			//===========================================================
			
			
			ZRGetForces(ctrlControl);				//Get user forces (sets initial value of ctrlControl forces)
			ZRGetTorques(&ctrlControl[TORQUE_X]);	//Get user torques (sets initial value of ctrlControl torques)
			
#ifdef ZRFLATFLOOR
			for (ii=0; ii<3; ii++){
				ctrlControl[ii] *= VEHICLE_MASS/4.3f; //Scale the force by the ratio of the current mass to the ISS mass
			}
#endif
			if (ZRGetPositionTarget(&ctrlStateTarget[POS_X])){
				clControl = 1;
				//Clear the original rate targets that were set in case no command was issued
				memset(&ctrlStateTarget[VEL_X],0,sizeof(float)*3);
			} else if (ZRGetVelocityTarget(&ctrlStateTarget[VEL_X])){
				clControl = 2;
			}
			userSetAtt = ZRGetAttitudeTarget(userAtt);
			if (userSetAtt){
				
				float currentAtt[3];
				ZRQuat2AttVec(refVector,&ctrlState[QUAT_1],currentAtt);	
				ZRAttVec2Quat(currentAtt,userAtt,&ctrlState[QUAT_1],&ctrlStateTarget[QUAT_1]);
				
				//Clear rate targets that were set
				memset(&ctrlStateTarget[RATE_X],0,sizeof(float)*3);
			} 
			
			
			//Standard state error function
			findStateError(ctrlStateError, ctrlState, ctrlStateTarget);
			
			//Enable attitude control only when user sets target
			if (userSetAtt){
				//Get attitude control
				ctrlAttitudeNLPIDwie(KPattitudePID, KIattitudePID, KDattitudePID, KPattitudePID, KIattitudePID, KDattitudePID, KPattitudePID, KIattitudePID, KDattitudePID, 1.0, ctrlStateError, ctrlControl);
			}
			//Only perform closed loop position control if user commands it.
			if (clControl) {
				switch (clControl){
					case 1:
						//Get position control
#ifndef ZRFLATFLOOR
						ctrlPositionPD(KPpositionPD, KDpositionPD, ctrlStateError, ctrlControl);
#else
						ctrlPositionPIDgains(1.5*KPpositionPID, KIpositionPID, KDpositionPID,1.5*KPpositionPID, KIpositionPID, KDpositionPID,1.5*KPpositionPID, KIpositionPID, KDpositionPID,1.0,ctrlStateError, ctrlControl);
#endif
						break;
					case 2:
						//Get velocity control
#ifndef ZRFLATFLOOR
						ctrlVelocityPgains(KPvelocityP, KPvelocityP, KPvelocityP, ctrlStateError, ctrlControl);
#else
						ctrlVelocityPgains(KPvelocityP, KPvelocityP, KPvelocityP, ctrlStateError, ctrlControl);
#endif
						break;
				}
			}
			//Repulsion algorithm
			ZRGetRepelForce(refVector, ctrlControl, ctrlState, ctrlStatePartner);
			
			//Collision avoidance for both satellites
			collisionActive=ctrlAvoidWithOverride(ctrlState,
												  &ctrlStatePartner,
												  1,
												  ctrlControl,
												  collisionInfo,3);
			
			enforceBoundaryLimits(ctrlState,ctrlControl);
			
			//Detect pick up and drop for torquing
			if (iHavePanel() && !lastHavePanel){ //picked up
				panelTorqueCounts = 12;
				torqueOnCounts = 2;
				panelTorqueDir = 1.0f;
			} else if (!iHavePanel() && lastHavePanel) { //dropped
				panelTorqueCounts = 12;
				torqueOnCounts = 2;
				panelTorqueDir = -1.0f;
			}
			//Victory roll at station (only will be called once since manuever transitions)
			if (reachedStation()){
				panelTorqueCounts = 1;
				torqueOnCounts = 1;
			}
			
			lastHavePanel = iHavePanel();
			
			if (panelTorqueCounts){
				panelTorqueCounts--;
				if (torqueOnCounts){
					torqueOnCounts--;
					ctrlControl[TORQUE_X] = panelTorqueDir*1.0f; //Full torque about x axis for 3 seconds
				} else {
					ctrlControl[TORQUE_X] = 0.0f;
				}
			}
			
			
			break;
		default:
			break;
	}
	
	//Activate thrusters
	if (maneuver_number > 1){
		ctrlMixWLoc(&firing_times, ctrlControl, ctrlState, minPulseWidth, duty_cycle, FORCE_FRAME_INERTIAL);		
#ifdef ZRFLATFLOOR
		//On the flat floor intercept forces and torques and use for simulation.  The x,y forces will steer
		//the satellite on the floor, while the remaining forces and torques will be used in simulation.
		ZRSimForceTorque(&firing_times, ctrlControl);
#endif
		
		//Set thruster times (only if not out of fuel)
		propSetThrusterTimes(&firing_times);
		
#if(SPHERE_ID == SPHERE1)
#ifdef ZRFLATFLOOR		
		padsGlobalPeriodSetAndWait(200,405);
#else
		padsGlobalPeriodSetAndWait(200,205);
#endif
#endif
	}
	
	//============= Termination Conditions ===================
	
	//ALWAYS terminate if the other satellite resets
	if (soh_partner.last_test_result == 255  ||
	    soh_partner.last_test_result == ZR_RESULT_INVALID_COMPETITOR) //Check background state of health for partner reset
	{
		//If a reset happened, end with an error.
		ctrlTestTerminate(TEST_RESULT_ERROR);
	}
	
	if (soh_partner.last_test_result >= 20 && soh_partner.last_test_result <= 39){ //Always terminate if partner has won
		ctrlTestTerminateZR(40 + teamId);
	}
	
	switch(maneuver_number)
	{
		case 2: //Initial position -> Game
			//Transition purely based on time so both satellites go into game at the same time
			if(maneuver_time > 30000){
				//Reset the game now that we are going into the game mode
				initZRGame();
				ZRInit();
				//Go to game mode
				ctrlManeuverTerminate();
			}
			break;
		case 3: // Termination Conditions
			/* 20 - 39	I won
			 * 40 - 59	They won
			 * 60 - 79	Timeout: I won
			 * 80 - 99	Timeout: They won
			 * 100 - 119  FuelDone: I won
			 * 120 - 139  FuelDone: I lost
			 * 140 - 159  Draw: FuelDone
			 * 160 - 179  Draw: Timeout
			 * 180 - 199  Draw: Docked
			 */
			
			// Timeout Conditions
			if(maneuver_time >= ZR_TIMEOUT) {
				if (getCurrentScore() > getOtherCurrentScore()){ //Timeout, win, higher score
					ctrlTestTerminateZR(60 + teamId);
				} else if (getCurrentScore() < getOtherCurrentScore()){ //Timeout, lost, lower score
					ctrlTestTerminateZR(80 + teamId);
				} else {
					ctrlTestTerminateZR(breakTieResult(80));
				}
			}
			
			//Out of fuel: wait for other satellite to terminate
			if (getFuelFlag()){
				ctrlManeuverNumSet(242);
			}
			
			//Reached station: go to tie check
			if (reachedStation()){
				setDockTime(maneuver_time/1000); //Use game time as dock time (only accurate to 1s since dock condition is checked once per second)
				ctrlManeuverNumSet(241);
			}
			break;
			
			
		case 241: //Satellite reaches station
			//Wait 2 seconds to sync game state in case of a tie
			if (maneuver_time >= 2000){
				unsigned char testResult;
				if (getDockTime() < getOtherDockTime()){
					testResult = 20 + teamId;
				} else if (getDockTime() == getOtherDockTime()) { // tie in dock time
					if (getCurrentScore() > getOtherCurrentScore()){ //Win by score
						testResult = 20 + teamId;
					} else if (getCurrentScore() < getOtherCurrentScore()){ //Lost by score
						testResult = 40 + teamId;
					} else { //Tie by score
						testResult = 180 + teamId;
					}
				} else {
					testResult = 40 + teamId;
				}
				ctrlTestTerminateZR(testResult);
			} 
			
			break;
		case 242: //Out of fuel, wait for other satellite to terminate
			if (soh_partner.last_test_result || soh_partner.maneuver_number == 242){
				unsigned char test_result;
				// FuelDone Conditions, or partner did not win
				if (getOtherFuelFlag() || soh_partner.last_test_result > 39){
					if (getCurrentScore() > getOtherCurrentScore()){
						test_result = 100 + teamId;
					} else if (getCurrentScore() < getOtherCurrentScore()){
						test_result = 120 + teamId;
					} else {
						test_result = breakTieResult(120);
					}
				} else {
					test_result = 120 + teamId;
				}
				ctrlTestTerminateZR(test_result);
			}
			break;
		default:
			break;
	}
	
#ifdef ZRFLATFLOOR
	//Get the actual estimated state so we can see if the estimator is converging
	padsStateGet(ctrlState);
#endif
	
	
	//These vectors are used for feedback to the user
	DebugVecShort[0] = (short) (test_time / 100 );
	for(ii=0; ii<3; ii++){
		DebugVecShort[ii+1] = (short) (1000.0f * ctrlStateTarget[ii]);
		DebugVecShort[ii+4] = (short) (ctrlControl[ii]*10000);
		DebugVecShort[ii+7] = (short) (userAtt[ii]*1000);
#ifdef ZRFLATFLOOR		
		DebugVecShort[ii+10] = (short) (ctrlState[QUAT_1+ii]*1000.0f);
		DebugVecShort[ii+13] = (short) (ctrlStateError[POS_X+ii]*1000.0f);
#endif
	}
	
	//First element always has test time
	DebugVecFloat[0] = (test_time/1000.0f);
	
	getPanelStateInternal(posPanel);
	//Send position of panel and other items
	DebugVecFloat[1] = getCurrentScore();
	DebugVecFloat[2] = ctrlControl[TORQUE_Y];
	DebugVecFloat[3] = ctrlControl[TORQUE_Z];
	DebugVecFloat[4] = posPanel[0];
	DebugVecFloat[5] = posPanel[1];
	DebugVecFloat[6] = posPanel[2];
	DebugVecFloat[7] = posPanel[3];
	
	//These vector components are for Panel Manipulations
	DebugVecUShort[0] = (unsigned short)(test_time/100);
	DebugVecUShort[1] = (unsigned short)iHavePanel();
	DebugVecUShort[2] = (unsigned short)otherHasPanel();
	DebugVecUShort[3] = (unsigned short)reachedStation();
	DebugVecUShort[4] = (unsigned short)getFuelFlag();
	DebugVecUShort[5] = (unsigned short)getDockTime();
	DebugVecUShort[6] = getUserRepelForce();
	DebugVecUShort[7] = getMaxFuelUse()/10;
	DebugVecUShort[8] = (unsigned short) getPercentFuelRemaining();
	DebugVecUShort[9] = (unsigned short) getPercentChargeRemaining();
	DebugVecUShort[10] = isPanelInSync();
	DebugVecUShort[11] = getPanelFoundCounts();
	DebugVecUShort[12] = outsideBoundary(ctrlState);
	DebugVecUShort[13] = collisionActive;
	DebugVecUShort[14] = teamId+1;
	DebugVecUShort[15] = getPanelDistScore();
	
	
	commSendPacket(COMM_CHANNEL_STL, GROUND, sysIdentityGet(), COMM_CMD_DBG_SHORT_SIGNED, (unsigned char *) DebugVecShort,0);
	commSendPacket(COMM_CHANNEL_STL, BROADCAST, sysIdentityGet(), COMM_CMD_DBG_FLOAT, (unsigned char *) DebugVecFloat,0);
	commSendPacket(COMM_CHANNEL_STL, BROADCAST, sysIdentityGet(), COMM_CMD_DBG_SHORT_UNSIGNED, (unsigned char *) DebugVecUShort,0);	
}

float sign(float a)
{
	if (a >= 0) return 1.0f;
	else return -1.0f;
}

void ctrlTestTerminateZR(unsigned char result){
	//Clear IR noise flag
	ctrlSetIRNoise(0);
	if (result > 254)
		result = 254;
	ctrlTestTerminate(result);
	
}
void gspProcessRXDataZR(default_rfm_packet packet)
{
	if (packet[PKT_CM] == COMM_CMD_DBG_SHORT_UNSIGNED)
	{
		dbg_ushort_packet DebugVecPanel;
		memcpy(DebugVecPanel, &packet[PKT_DATA], sizeof(dbg_ushort_packet));
		setOtherPanel((unsigned char)DebugVecPanel[1]);
		setOtherReachedStation((unsigned char)DebugVecPanel[3]);
		setOtherFuelFlag((unsigned char)DebugVecPanel[4]);
		setOtherDockTime((unsigned int)DebugVecPanel[5]);
		setOtherRepelForce(DebugVecPanel[6]);
		setOtherPanelFound(DebugVecPanel[11]);
		setOtherPanelDistScore(DebugVecPanel[15]);
		
	}
	if (packet[PKT_CM] == COMM_CMD_DBG_FLOAT)
	{
		dbg_float_packet DebugVecFloat;
		memcpy(DebugVecFloat, &packet[PKT_DATA], sizeof(dbg_float_packet));
		setOtherCurrentScore(DebugVecFloat[1]);
	}	
}

void assignFcnPtrs(unsigned char id){
	
	if (!fcnsAssigned){ //Only assign if not already set
		ZRUser = getZRUserFunctionByIndex(id);
		ZRInit = getZRInitFunctionByIndex(id);
		if (ZRUser == 0){
			ctrlSetIRNoise(0);
			ctrlTestTerminate(ZR_RESULT_INVALID_COMPETITOR);
		}
		teamId = id;
	}
	fcnsAssigned = 1;
}

unsigned char breakTieResult(unsigned char defaultVal){
	
	unsigned char result = defaultVal+teamId;
	
	if (getPanelFoundCounts() > getOtherPanelFoundCounts()){
		result = 140+teamId;
	} else if (getPanelFoundCounts() == getOtherPanelFoundCounts()){
		if (getPanelDistScore() < getOtherPanelDistScore()){
			result = 160+teamId;
		}
	}
	return result;
}
